
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_ahrs.c
  * @author     baiyang
  * @date       2021-8-23
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       是否稳定定位
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
bool fms_position_ok()
{
    return ahrs_valid_lpos(fms.ahrs);
}

/**
  * @brief       ekf是否有相对定位
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
bool fms_ekf_has_relative_position()
{
    return false;
}

/**
  * @brief       高度是否可用
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
bool fms_ekf_alt_ok()
{
    return ahrs_valid_alt(fms.ahrs);
}

// update_auto_armed - update status of auto_armed flag
void fms_update_auto_armed()
{
    // disarm checks
    if(fms.ap.auto_armed){
        // if motors are disarmed, auto_armed should also be false
        if(!fms.motors->_armed) {
            fms_set_auto_armed(false);
            return;
        }
        // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
        if(mode_has_manual_throttle(fms.flightmode) && fms.ap.throttle_zero && !fms.failsafe.radio) {
            fms_set_auto_armed(false);
        }

    }else{
        // arm checks
        
        // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
        if(fms.motors->_armed && fms.ap.using_interlock) {
            if(!fms.ap.throttle_zero && fms.motors->_spool_state == MOTOR_THROTTLE_UNLIMITED) {
                fms_set_auto_armed(true);
            }
        // if motors are armed and throttle is above zero auto_armed should be true
        // if motors are armed and we are in throw mode, then auto_armed should be true
        } else if (fms.motors->_armed && !fms.ap.using_interlock) {
            if(!fms.ap.throttle_zero || mode_number(fms.flightmode) == THROW) {
                fms_set_auto_armed(true);
            }
        }
    }
}

// rotate vector from vehicle's perspective to North-East frame
void fms_rotate_body_frame_to_NE(float *x, float *y)
{
    float ne_x = (*x) * fms.ahrs->cos_yaw - (*y) * fms.ahrs->sin_yaw;
    float ne_y = (*x) * fms.ahrs->sin_yaw + (*y) * fms.ahrs->cos_yaw;
    *x = ne_x;
    *y = ne_y;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_update_ahrs()
{
    ahrs_view_update();

    fms_update_home_from_EKF();

    // pull position from ahrs
    Location loc;
    ahrs_get_location(fms.ahrs, &loc);
    fms.current_loc.lat = loc.lat;
    fms.current_loc.lng = loc.lng;

    // exit immediately if we do not have an altitude estimate
    if (!fms.ahrs->valid_alt) {
        return;
    }

    // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin
    const int32_t alt_above_origin_cm = ahrs_get_position_z_up_cm(fms.ahrs);
    location_set_alt_cm(&fms.current_loc, alt_above_origin_cm, ALT_FRAME_ABOVE_ORIGIN);

    if (!ahrs_home_is_set(fms.ahrs) || !location_change_alt_frame(&fms.current_loc, ALT_FRAME_ABOVE_HOME)) {
        // if home has not been set yet we treat alt-above-origin as alt-above-home
        location_set_alt_cm(&fms.current_loc, alt_above_origin_cm, ALT_FRAME_ABOVE_HOME);
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_ahrs_init()
{
    ahrs_view_init();
}
/*------------------------------------test------------------------------------*/


